﻿import time
# import ctypes
from  base_mpu6050 import mpu_object
import numpy as np
# from scipy.stats import linregress
import matplotlib.pyplot as plt


mpu = mpu_object()

# 卡尔曼滤波器类
class KalmanFilter:
    def __init__(self, Q=1e-5, R=0.01):
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 测量噪声协方差
        self.x = 0.0  # 初始估计值
        self.P = 1.0  # 初始估计协方差
        self.K = 0.0  # 初始卡尔曼增益

    def update(self, measurement):
        # 预测步骤
        self.P = self.P + self.Q  # 更新估计协方差
        # 计算卡尔曼增益
        self.K = self.P / (self.P + self.R)
        # 更新估计值
        self.x = self.x + self.K * (measurement - self.x)
        # 更新协方差
        self.P = (1 - self.K) * self.P
        return self.x


# 初始化卡尔曼滤波器
kf = KalmanFilter()

# 存储读取的数据
data = []
timestamps = []
start_time = time.time()


# 模拟读取传感器数据的函数，这里用随机数生成6个浮点数
def read_sensor_data():
    a_x,a_y,a_z = mpu.read_accel()
    g_x,g_y,g_z =mpu.read_gyro()
    return a_x,a_y,a_z,g_x,g_y,g_z


avr_value = 0
count = 0
# 读取数据，每0.1秒读取一次，持续10分钟
while (time.time() - start_time) < 600:  # 10分钟为600秒
    sensor_data = read_sensor_data()
    current_time = time.time() - start_time
    # timestamps.append(current_time)  # 记录时间戳
    # data.append(sensor_data)  # 记录传感器数据
    raw_value = sensor_data[5]
    
    # 使用卡尔曼滤波器平滑数据
    filtered_value = kf.update(raw_value)
    avr_value += filtered_value
    data.append([current_time,sensor_data[5],filtered_value])
    count+=1
    time.sleep(0.1)  # 每0.1秒
    
    # print(filtered_value)



#遍历data
# for i in range(len(data)):
#     print(data[i])


# 将数据转换为NumPy数组，方便后续处理
data = np.array(data)
# # 绘制每个传感器数据的时间序列图
time_stamps = data[:, 0]

# # 线性拟合每一列数据（6个传感器数据集），x为时间，y为读取的数据
# coefficients = []
# 
# # 拟合每个传感器数据列，生成y = kx + b的函数表达式
# for i in range(1, 7):  # 数据的第1列是时间，第2到7列是传感器数据
#     x = data[:, 0]  # 时间列
#     y = data[:, i]  # 传感器数据列
#     slope, intercept, r_value, p_value, std_err = linregress(x, y)
#     print(f"Sensor {i}: y = {slope:.4f}x + {intercept:.4f}")


avr_value = avr_value/count
print("avr_value:",avr_value)
plt.figure(figsize=(10, 8))
for i in range(1, 3):  # 数据的第1列是时间，第2到7列是传感器数据
    plt.plot(time_stamps, data[:, i], label=f'Sensor {i}')

plt.xlabel('Time (s)')
plt.ylabel('Sensor Values')
plt.title('Sensor Data Over Time')
plt.legend()
plt.grid(True)
plt.show()






